#include "CAN.h"

#define MSG_RX 0
#define MSG_TX 10

extern const short msgId_CandIdReceived;
extern const short msgId_ActuPosition;
extern const short msgId_ActuAngle;
extern const short msgId_ArretPosition;
extern const short msgId_ArretAngle;
extern const short msgId_InfoEntraxe;
extern const short msgId_Angle;
extern boolean Arret;

extern unsigned long int Xpos,Ypos;
extern double TetaPos;
extern uint16 EntraxePasG, EntraxePasD;
extern uint16 u16MaxCommande;
extern uint16 u16MaxCommandeRot;
extern uint16 u16Pente;
extern byte PIDSampleTime;
extern uint16 u16PasPente;
extern double u16RadPente;
extern long int AnglePrecision;

#define msgId_AvancePosition 101
#define msgId_AvancePositionBack 104
#define	msgId_AvanceAngle 102
#define	msgId_Stop 103
#define	msgId_DemandePosition 111
#define	msgId_EtatCapteur 121
#define	msgId_SetPositionX 130
#define	msgId_SetPositionY 131
#define	msgId_SetPositionTeta 132
#define msgId_DemandeOuvertureCharriotDroite 140 //Demande d ouverture du charriot sur le cote droit du robot
#define msgId_DemandeOuvertureCharriotGauche 141 // Demande d ouverture du charriot sur le cote gauche du robot
#define msgId_DemandeOuvertureCharriotMilieu 142

#define msgId_InitEntraxe 50	// initialisation des valeurs de l entraxe
#define msgId_InitPente 53	// initialisation des valeurs de la pente
#define msgId_InitVitesseMax 54	// initialisation des valeurs de la vitesse max
#define msgId_InitPid1 51		// initialisaton des valeurs du PID vitesse du moteur 1
#define msgId_InitPid2 52		// initialisaton des valeurs du PID vitesse du moteur 2



void CanInit(void)
{
 	sint8 s8Error;
	init_can(250);
	TCanObjConfig tCanObj;
	tCanObj.u32Id = 1;
	tCanObj.u8Xtd = false;
	tCanObj.u8MsgType = TX;
	tCanObj.u8NumberMsgs = 1;
	s8Error |= config_can_obj(MSG_TX, CAN_GLOB_MASK_SHORT, CAN_GLOB_MASK_LONG, &tCanObj);

	tCanObj.u32Id = 1;
	tCanObj.u8Xtd = false;
	tCanObj.u8MsgType = RX;
	tCanObj.u8NumberMsgs = 10;
	s8Error |= config_can_obj(MSG_RX, 0, 0, &tCanObj);
	
	    
}

void CanSendString(char message[])
{
	T_Rx_Tx tCanMsg;
	sint8 s8Error;
	uint8 j=0;
	uint8 i;
	tCanMsg.u8Xtd = false;
	tCanMsg.u32Id =1;
	while (j<strlen(message))
	{
		if ((strlen(message)-j)>8)
		  tCanMsg.u8Dlc =8;
		else
			tCanMsg.u8Dlc=strlen(message)-j;

		//tCanMsg.u8Dlc = (strlen(message)-j)>8 ? 8 : (strlen(message)-j) ;
	
		for (i=0;i<tCanMsg.u8Dlc;i++)
		{
			tCanMsg.au8Data[i] = message[i+j];
		}
		j=j + 8 ;
		
		

		do {
			s8Error = send_can_msg(10, &tCanMsg);
		} while (s8Error != C_NO_ERR);
	}
	
}

void CanSendInt16(uint16 commande, uint16 mesure, uint16 asserv, uint16 counter, short msgId)
{
	T_Rx_Tx tCanMsg;
	sint8 s8Error;
	tCanMsg.u8Xtd = false;
	tCanMsg.u32Id = msgId;



	tCanMsg.au8Data[0] = commande & 0x00FF;
	tCanMsg.au8Data[1] = commande >> 8;

	tCanMsg.au8Data[2] = mesure & 0x00FF;
	tCanMsg.au8Data[3] = mesure >> 8;

	tCanMsg.au8Data[4] = asserv & 0x00FF;
	tCanMsg.au8Data[5] = asserv >> 8;

	tCanMsg.au8Data[6] = counter & 0x00FF;
	tCanMsg.au8Data[7] = counter >> 8;
	
	tCanMsg.u8Dlc = 8;


	do {
		s8Error = send_can_msg(10, &tCanMsg);
	} while (s8Error != C_NO_ERR);

	
}
 
void CanSendInt(long int left, long int right, short msgId)
{
	T_Rx_Tx tCanMsg;
	sint8 s8Error;
	tCanMsg.u8Xtd = false;
	tCanMsg.u32Id = msgId;

	tCanMsg.au8Data[0] = left;
	tCanMsg.au8Data[1] = left >> 8 ;
	tCanMsg.au8Data[2] = left >> 16;
	tCanMsg.au8Data[3] = left >> 24;

	tCanMsg.au8Data[4] = right;
	tCanMsg.au8Data[5] = right >> 8;
	tCanMsg.au8Data[6] = right >>16;
	tCanMsg.au8Data[7] = right >> 24;

	tCanMsg.u8Dlc = 8;


	do {
		s8Error = send_can_msg(10, &tCanMsg);
	} while (s8Error != C_NO_ERR);
}


void CanSendDouble(double double1,short msgId)
{
/*	T_Rx_Tx tCanMsg;
	sint8 s8Error;
	tCanMsg.u8Xtd = false;
	tCanMsg.u32Id = msgId;

	tCanMsg.au8Data[0] = double1;
	tCanMsg.au8Data[1] = double1 >> 8;
	tCanMsg.au8Data[2] = double1 >> 16;
	tCanMsg.au8Data[3] = double1 >> 24;
	tCanMsg.au8Data[4] = double1 >> 32
	tCanMsg.au8Data[5] = double1 >> 40;
	tCanMsg.au8Data[6] = double1 >> 48;
	tCanMsg.au8Data[7] = double1 >> 56;

	tCanMsg.u8Dlc = 8;


	do {
		s8Error = send_can_msg(10, &tCanMsg);
	} while (s8Error != C_NO_ERR);
*/	
}


void CanReceive(void)//uint8 *au8Data[8])
{
	sint8 s8Error;
	T_Rx_Tx tCanMsg;
	unsigned long int msgId;
	s8Error = get_can_msg(MSG_RX, &tCanMsg);
	if (s8Error == C_NO_ERR)
	{  
		msgId = tCanMsg.u32Id  ;
		//CanSendString("Received#");
		//CanSendInt(msgId,0,msgId_CandIdReceived);

		switch (msgId) {

			case (msgId_AvancePosition):
				DeplaceAvancePosition(ConvertByteToLong(tCanMsg.au8Data,0),ConvertByteToLong(tCanMsg.au8Data,4));
				break;
			case (msgId_AvancePositionBack):
				DeplaceAvancePositionMarcheArriere(ConvertByteToLong(tCanMsg.au8Data,0),ConvertByteToLong(tCanMsg.au8Data,4));
				break;

			case (msgId_AvanceAngle) :
				DeplaceAvanceAngle(((double)ConvertByteToULong(tCanMsg.au8Data,0))/((double)AnglePrecision));
				//CanSendInt(628318,9999999,msgId_Angle);
				CanSendInt(ConvertByteToULong(tCanMsg.au8Data,0),666,msgId_Angle);
				break;

			case (msgId_Stop):
				DeplaceStop();
				break;
			
			case (msgId_DemandePosition):
				if(Arret)
				{
					CanSendInt(Xpos,Ypos,msgId_ArretPosition);
					CanSendInt(TetaPos * AnglePrecision,0,msgId_ArretAngle);
				}
				else
				{
					CanSendInt(Xpos,Ypos,msgId_ActuPosition);
					CanSendInt(TetaPos * AnglePrecision,0,msgId_ActuAngle);
				}
				break;

			case (msgId_InitPid1):
				AsservInitPid1(ConvertByteToInt16(tCanMsg.au8Data,0),ConvertByteToInt16(tCanMsg.au8Data,2),ConvertByteToInt16(tCanMsg.au8Data,4));
				CanSendString("IniPID1#");
				break;

			case (msgId_InitPid2):
				AsservInitPid2(ConvertByteToInt16(tCanMsg.au8Data,0),ConvertByteToInt16(tCanMsg.au8Data,2),ConvertByteToInt16(tCanMsg.au8Data,4));
				CanSendString("IniPID2#");
				break;

			case (msgId_InitEntraxe):
				AsservInitEntraxe( ConvertByteToLong(tCanMsg.au8Data,0), ConvertByteToLong(tCanMsg.au8Data,4));
				CanSendString("DefEntr#");
				CanSendInt(EntraxePasD,EntraxePasG,msgId_InfoEntraxe);
				break;

			case (msgId_InitPente):
				u16Pente= ConvertByteToLong(tCanMsg.au8Data,0);
				u16PasPente = ((double)u16MaxCommande *  (double)u16MaxCommande *  (double)PIDSampleTime) /(2000* (double)u16Pente);
				u16RadPente = (((double)u16MaxCommandeRot * (double)u16MaxCommandeRot * (double)PIDSampleTime) /(2000 * (double)u16Pente)) * 2 / (double)((EntraxePasD + EntraxePasG) / 2);
				break;

			case (msgId_InitVitesseMax):
				u16MaxCommande = ConvertByteToLong(tCanMsg.au8Data,0);
				u16PasPente = ((double)u16MaxCommande *  (double)u16MaxCommande *  (double)PIDSampleTime) /(2000* (double)u16Pente);
				break;
			case (msgId_SetPositionX):
				Xpos = ConvertByteToLong(tCanMsg.au8Data,0);
				break;
			case (msgId_SetPositionY):
				Ypos = ConvertByteToLong(tCanMsg.au8Data,0);
				break;
			case (msgId_SetPositionTeta):
				TetaPos = (double)ConvertByteToLong(tCanMsg.au8Data,0) / (double)100000;
				break;
			case (msgId_EtatCapteur):
				CapteursGet();
				break;
			case (msgId_DemandeOuvertureCharriotDroite):
				CapteursOuvreCharriotADroite();
				break;
			case (msgId_DemandeOuvertureCharriotGauche):
				CapteursOuvreCharriotAGauche();
				break;
			case (msgId_DemandeOuvertureCharriotMilieu):
				CapteursFermeCharriotMilieu();
				break;
/*
#define	msgId_EtatCapteur 121
*/
			default:
				CanSendString("MsgErro#");
				break;


		}

	}
}

long int ConvertByteToLong(uint8 data[],short offset)
{
	//return ((int)data[0+offset] + ((int)data[1+offset]<<8) + ((int)data[2+offset]<<16) + ((int)data[3+offset]<<24));
	long int temp;
	memcpy(&temp,data+offset, sizeof(long int));
	return temp;
}

unsigned long int ConvertByteToULong(uint8 data[],short offset)
{
	//return (data[0+offset] | (data[1+offset]<<8) | (data[2+offset]<<16) | (data[3+offset]<<24));
	unsigned long int temp;
	memcpy(&temp,data +offset , sizeof(unsigned long int));
	return temp;
}

int ConvertByteToInt16(uint8 data[],short offset)
{
	//return ((int)data[0+offset] + ((int)data[1+offset]<<8) + ((int)data[2+offset]<<16) + ((int)data[3+offset]<<24));
	int temp;
	memcpy(&temp,data+offset, sizeof(int));
	return temp;
}
